#ifndef _TRAJECTORYPREDICTOR_H
#define _TRAJECTORYPREDICTOR_H

#include <iostream>
#include <vector>
#include "obstacle_detector/util.h"


namespace obstacle_detector
{
class TrajectoryPredictor
{
public:
	TrajectoryPredictor(bool debug);
	~TrajectoryPredictor(){};
	void get_predicted_trajectory(std::vector<Object> &objects,
		const std::vector<float> &roads_yaw);

private:
	void project_predict_locs(const Eigen::Vector2f &center,float yaw,
		Eigen::MatrixXf &predict_locs);
	void get_polynominal_values();
	bool is_stop(const std::vector<Eigen::Vector2f> &history_v);
	void curve_fitting(const Eigen::VectorXf &x, 
		const Eigen::VectorXf &y,Eigen::VectorXf &p_x,
		Eigen::VectorXf &p_y,int order,int start_index1,
		int start_index2);

private:
	bool debug_;

	const int predict_num_=61;
	const int polynominal_order_=3; //切比雪夫多项式级数
	Eigen::MatrixXf polynominal_values_;


};//class

}

#endif